
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.ArrayList;
import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.TachoPilot;


public class BarCodeFSM extends Thread {
	
	static LightSensor sensor = new LightSensor(SensorPort.S1);
	static final TachoPilot pilot = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C, true);
	// samplingPeriod in milliseconds
	static final int samplingPeriod = 10;
	static final int numBufferSamples = 3;
	
	int[] signalBuffer;
	int[] barcode = new int[7];
	static ArrayList<Integer> stateDurations = new ArrayList<Integer>();
	static boolean stopMe;
	
	boolean readingStrip;
	static int state;
	static int prevState;
	static int BROWN = 508;
	static int WHITE = 459;
	static int BLACK = 684;
	static int EDGE = 0;
	static int Vbz;
	static int Vwb;
	private static BTConnection connection;
	private static DataInputStream dataIn;
	private static DataOutputStream dataOut;

	public BarCodeFSM() {
		// roep constructor van superclass aan ( Thread() )
		super();
		signalBuffer = new int[numBufferSamples];
		readingStrip = false;
		state = BROWN;
	}
	
	public static void main(String[] args)
	{	
		BarCodeFSM reader = new BarCodeFSM();
		BarCodeFSM.sensor.setFloodlight(true);
//		connect();
		RConsole.openBluetooth(0);
		
		System.out.println("Press ENTER to calibrate BLACK");
		Button.ENTER.waitForPressAndRelease();
		BLACK = SensorPort.S1.readRawValue();
		System.out.println(BLACK);
		
		
		System.out.println("Press ENTER to calibrate WHITE");
		Button.ENTER.waitForPressAndRelease();
		WHITE = SensorPort.S1.readRawValue();
		System.out.println(WHITE);
		
		System.out.println("Press ENTER to calibrate BROWN");
		Button.ENTER.waitForPressAndRelease();
		BROWN = SensorPort.S1.readRawValue();
		System.out.println(BROWN);
		
		Vbz = (BROWN + BLACK) / 2;
		Vwb = (WHITE + BROWN) / 2;
		
		System.out.println("Press ENTER to start driving");
		Button.ENTER.waitForPressAndRelease();
		LCD.clear();
		pilot.forward();
		reader.fillSignalBuffer(BROWN);
		state = BROWN;
		reader.start();
		
		Button.ENTER.waitForPressAndRelease();
		stopMe = true;
		pilot.stop();
		LCD.clear();
		// prints the barcode on screen
		//reader.interpretBarcode();
		System.out.println("Pilot stopped \n Press ENTER to connect");
		
		Button.ENTER.waitForPressAndRelease();
		connection.close();
		System.out.println("file stream closed");

	}

	public void run(){
		try {
			
			for (;;) {
				Thread.sleep(samplingPeriod);
				int lightValue = SensorPort.S1.readRawValue();	
				push(lightValue);
				int gradient = signalBuffer[signalBuffer.length - 1] - signalBuffer[signalBuffer.length - 2];
				
				boolean onEdge = gradient >= 18 || gradient <= -18;
				boolean lightBrown = (lightValue <= Vbz) && (lightValue > Vwb);
				boolean lightWhite = (lightValue <= Vwb);
				boolean lightBlack = (lightValue > Vbz);
				
				// switch statement werkt niet want BROWN,... zijn geen constanten
				prevState = state;
				if (state == BROWN) {
					if (onEdge || lightBlack || lightWhite) {
						readingStrip = true;
						state = EDGE;
//					} else if (lightWhite) { // dit zou normaal niet mogen voorvallen
//						readingStrip = true;
//						state = WHITE;
//					} else if (lightBlack) { // dit zou normaal niet mogen voorvallen
//						readingStrip = true;
//						state = BLACK;
					} else {
						readingStrip = false;
						state = BROWN;
					}
				} else if (state == WHITE) {
					if (onEdge || lightValue > Vwb) {
						state = EDGE;
//					} else if (lightBlack) { // dit zou normaal niet mogen voorvallen
//						readingStrip = true;
//						state = BLACK;
//					} else if (lightBrown) { // dit zou normaal niet mogen voorvallen
//						readingStrip = false;
//						state = BROWN;
					} else {
						readingStrip = true;
						state = WHITE;
					}
				} else if (state == BLACK) {
					if (onEdge || lightValue < Vbz) {
						state = EDGE;
//					} else if (lightWhite) { // dit zou normaal niet mogen voorvallen
//						readingStrip = true;
//						state = WHITE;
//					} else if (lightBrown) { // dit zou normaal niet mogen voorvallen
//						readingStrip = false;
//						state = BROWN;
					} else {
						readingStrip = true;
						state = BLACK;
					}
				} else if (state == EDGE) {
					if (!onEdge && lightBrown) {
						readingStrip = false;
						interpretBarcode();
						// clean up the old ArrayList
						stateDurations = new ArrayList<Integer>();
						state = BROWN;
					} else if (!onEdge && lightWhite) {
						readingStrip = true;
						state = WHITE;
					} else if (!onEdge && lightBlack) {
						readingStrip = true;
						state = BLACK;
					} else {
						state = EDGE;
					}
				}
				
				
//				try {
//					dataOut.writeInt(lightValue);
//					dataOut.writeInt(gradient);
//					dataOut.writeInt(state);
//					dataOut.flush();
//				} catch (IOException e) {
//			
//				}
				
				if(readingStrip) {
					if (state == prevState) {
						int lastElement = stateDurations.get(stateDurations.size() - 1);
						stateDurations.remove(stateDurations.size() - 1);
						stateDurations.add(lastElement - 1);
					} else {
						stateDurations.add(state);
						stateDurations.add(-1);
					}
				}
				// Don't Stop Me Now!!!!
				// Don't Stop Me Now!!!!
				// Cause i'm having a good time, having a good time...
				if(stopMe)	break;
			}
			
		} catch (InterruptedException e) {
			LCD.clear();
			System.out.println("run() interrupted");
		}
	}
	
	private void push(int lightValue)
	    {
			for (int i = 0; i < signalBuffer.length-1; i++) {
				signalBuffer[i] = signalBuffer[i + 1];
			}
			signalBuffer[signalBuffer.length-1] = lightValue;
			// Low pass filter: zorgt voor langere reactietijd
			//signalBuffer[signalBuffer.length-1] = (int) ((0.25 * lightValue) + (1 - 0.25) * signalBuffer[signalBuffer.length-1]);
	    }

	private void interpretBarcode()
	{
		for (int i = 0; i < stateDurations.size(); i++) {
			RConsole.println("stateDurations("+i+") : " + stateDurations.get(i));	
		}
		RConsole.println("EDGES VERWIJDEREN:");
		// ============== EDGE states verwijderen en totale duration van barcode bepalen ====================
		int duration = 0;
		ArrayList<Integer> tempstateDurations = new ArrayList<Integer>();
		for (int i = 0; i < stateDurations.size(); i++) {
			RConsole.println("working on " + i + " = " + stateDurations.get(i));
			int curState = stateDurations.get(i);
			if ( curState > 0) {
				// negatieve durations !!!
				duration -= stateDurations.get(i + 1);
			} else if (curState == EDGE) {
				int edgeDur = -stateDurations.get(i + 1);
				int lower = edgeDur / 2;
				int upper = edgeDur - lower;
				if (i - 1 >= 0) {
					int updatedDurPrev = stateDurations.get(i - 1) - lower;
					stateDurations.set(i - 1, updatedDurPrev);
				}
				if (i + 3 < stateDurations.size()) {
					int updatedDurNext = stateDurations.get(i + 3) - upper;
					stateDurations.set(i + 3, updatedDurNext);
				}
				//want was er nog niet bijgeteld, upper wordt later bijgeteld:
				duration += lower;
			}
		}
		for (int i = 0; i < stateDurations.size(); i+=2) {
			if(stateDurations.get(i)!= EDGE) {
				tempstateDurations.add(stateDurations.get(i));
				tempstateDurations.add(stateDurations.get(i+1));
			}
		}
		stateDurations = tempstateDurations;
		for (int i = 0; i < stateDurations.size(); i++) {
			RConsole.println("stateDurations("+i+") : " + stateDurations.get(i));	
		}
		RConsole.println("duration = " + duration);
		float barDuration = duration / 7;
		RConsole.println("barduration = " + barDuration);
		// ============== streepjesLengte toewijzen aan elke state WHITE/BLACK ==================-
		int[][] stateAttribs = new int[7][3];
		int arrIndex = 0;
		for (int i = 0; i < stateDurations.size() && arrIndex < 7; i++) {
			int curState = stateDurations.get(i);
			if (curState > 0) {
				// echte lengte van deze state
				// als curState > 0 is er altijd een volgend element (nl de duration)
				int stateDur = -stateDurations.get(i + 1);
				// verschil tussen toegewezen streepjeslengte en echte lengte
				int rem = duration;
				// toegewezen streepjeslengte
				int assignedLength = 0;
				// vergelijk echte lengte met alle veelvouden van 1 gemiddelde streepjeslengte
				for (int j = 1; j <= 7; j++) {
					// currentRem < 0 betekent stateDur > j * barDuration
					// currentRem > 0 betekent stateDur < j * barDuration
					int currentRem = ((int) (j * barDuration)) - stateDur;
					if (Math.abs(currentRem) < Math.abs(rem)) {
						// verschil met toegewezen streepjeslengte
						rem = currentRem;
						// toegewezen streepjeslengte
						assignedLength = j;
					}
				}
				stateAttribs[arrIndex][0] = curState;
				stateAttribs[arrIndex][1] = rem;
				stateAttribs[arrIndex][2] = assignedLength;
				RConsole.println("Curstate/rem/assignedLength: " + curState + " " + rem + " " + assignedLength);
				arrIndex++;
			}
		}
		

		if (arrIndex <= 6) {
			//  maak state negatief om einde aan te geven
			stateAttribs[arrIndex][0] = -1;
		}
		
		// ================ check of totale streepjeslengte gelijk aan 7 en corrigeer indien nodig ==================
		int assignedLengthsSum = 0;
		for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
			assignedLengthsSum += stateAttribs[i][2];
		}
		if (assignedLengthsSum > 7) {
			RConsole.println("groter dan 7");
			while (assignedLengthsSum != 7) {
				// zoek die state(index) met rem > 0 en grootste rem (dus echte lengte korter dan toegewezen en het minst dicht bij toegewezen lengte)
				int largestRem = 0;
				int wantedStateIndex = 7;
				for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
					if (stateAttribs[i][1] > largestRem) {
						largestRem = stateAttribs[i][1];
						wantedStateIndex = i;
					}
				}
				RConsole.println("wantedstateindex " + wantedStateIndex);
				// assert dat hij er een gevonden heeft
				assert wantedStateIndex != 7;
				// maak toegewezen streepjeslengte 1 korter
				stateAttribs[wantedStateIndex][2] -= 1;
				// pas rem aan
				stateAttribs[wantedStateIndex][1] = (int) -(barDuration - largestRem);
				assignedLengthsSum = 0;
				for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
					assignedLengthsSum += stateAttribs[i][2];
				}
			}
		} else if (assignedLengthsSum < 7) {
			RConsole.println("kleiner dan 7");
			while (assignedLengthsSum != 7) {
				// zoek die state(index) met rem < 0 en kleinste rem (dus echte lengte langer dan toegewezen en het minst dicht bij toegewezen lengte)
				int smallestRem = 0;
				int wantedStateIndex = 7;
				for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
					if (stateAttribs[i][1] < smallestRem) {
						smallestRem = stateAttribs[i][1];
						wantedStateIndex = i;
					}
				}
				RConsole.println("wantedstateindex " + wantedStateIndex);
				// assert dat hij er een gevonden heeft
				assert wantedStateIndex != 7;
				// maak toegewezen streepjeslengte 1 langer
				stateAttribs[wantedStateIndex][2] += 1;
				// pas rem aan
				stateAttribs[wantedStateIndex][1] = (int) barDuration + smallestRem; // smallest rem negatief
				assignedLengthsSum = 0;
				for (int i = 0; i < 7 && stateAttribs[i][0] > 0; i++) {
					assignedLengthsSum += stateAttribs[i][2];
				}
			}
		}

		LCD.clear();
		System.out.print("The barcode is " );
		int k = 0;
		for (int i = 0; k < 7;i++) {
			for (int j = 0; j<stateAttribs[i][2]; j++) {
				System.out.print(stateAttribs[i][0]==BLACK? 1 : 0);
				k++;
			}
		}
	}

		// zie http://en.wikipedia.org/wiki/Finite_difference_coefficients
		private int finiteDifference() {
			// voorwaartse differentie over 4 punten
	//		float difference = - 11 * signalBuffer[signalBuffer.length - 4]
	//		                   + 18 * signalBuffer[signalBuffer.length - 3]
	//		                   - 9 * signalBuffer[signalBuffer.length - 2]
	//		                   + 2 * signalBuffer[signalBuffer.length - 1];
	//		return Math.round(difference / (samplingPeriod * 6));
			// voorwaartse differentie over 3 punten
	//		float difference = - 3 * signalBuffer[signalBuffer.length - 3]
	//		                   + 4 * signalBuffer[signalBuffer.length - 2]
	//		                   - 1 * signalBuffer[signalBuffer.length - 1];
	//		return Math.round(difference / (samplingPeriod * 2));
			return signalBuffer[signalBuffer.length - 1] - signalBuffer[signalBuffer.length - 2];
		}

	private int bufferMean()
    {
    	int sum = 0;  // sum of all the elements
    	for (int i = 0; i < signalBuffer.length; i++) {
    		sum += signalBuffer[i];
		}
        return sum / signalBuffer.length;

    }
	
	public void fillSignalBuffer(int value)
	{
		for (int i = 0; i < signalBuffer.length; i++) {
			signalBuffer[i] = value;
		}
	}
	
	/**
	 * Estabish bluetooth connection to mission control
	 */
	public static void connect()
	{
		LCD.clear();
		LCD.drawString("Waiting", 0, 0);
		connection = Bluetooth.waitForConnection(); // this method is very patient. 
		LCD.clear();
		LCD.drawString("Connected", 0, 0);
		dataIn = connection.openDataInputStream();
		dataOut = connection.openDataOutputStream();


	}
}